#ifndef COOLPOOL_SIMULATION_SIMOBJECTS_RIGIDBODYRESTINGCONTACTHANDLER_H_
#define COOLPOOL_SIMULATION_SIMOBJECTS_RIGIDBODYRESTINGCONTACTHANDLER_H_

#include <eigen3/Eigen/Dense>
#include <limits>
#include <MatrixTools.h>
#include <set>
#include <vector>
#include <Vector3D.h>

#include "Constants.h"
#include "Collision.h"
#include "EigenTools.h"
#include "RigidBody.h"
#include "RestingContactHandler.h"
#include "RigidBodyFriction.h"
#include "SimObjectsExceptions.h"

namespace matrix_tools = math_tools::matrix_tools;

namespace coolpool
{
	// Resting contact handler for rigid bodies, it is a bit complex so for details see
	// project references or documentation.
	class RigidBodyRestingContactHandler: public RestingContactHandler
	{
	public:

		RigidBodyRestingContactHandler();
		virtual ~RigidBodyRestingContactHandler();

		// Overrides:
		virtual void handleRestingContact(const std::vector<const Collision *> * resting_contacts);

	protected:

		// Computes one sell of A matrix.
		double computeAij(const Collision * ci, const Collision * cj);

		// Builds matrix A for system of equations Ax+b=0, it contains scales, which describes how
		// much one force affects the colliding bodies.
		const Eigen::MatrixXd * computeAMatrix(const vector<const Collision *> * resting_contacts);

		// Computes the forces acting between the two bodies of collision, while
		// one of them is static. It is possible to use the dynamic version, this
		// is just for optimalization purposes.
		double computeBElementStatic(const Collision * collision);

		// General method to compute forces acting between the two elements of collision.
		double computeBElementDynamic(const Collision * collision);

		// Computes all the forces between all the collisions.
		Eigen::VectorXd * computeBVector(const vector<const Collision *> * resting_contacts);

		// Computes the change of the collision normal.
		inline math_tools::Vector3D computeNormalChange(const Collision * col);

		// Increases the f_[d] until the b_[d] hits zero.
		// One iteration of algorithm.
		void driveToZero(int d);

		// Computes the direction of the resting force.
		Eigen::VectorXd fdirection(int d);

		// Finds index of interpenetrating bodies, the forces acting on these needs to be resolved.
		// It is called on the end of all the iteration. The algorithm ends when it returns -1.
		int findLessThanZero(Eigen::VectorXd * a);

		// Computes the maximum scale of the force computed by fdirection.
		std::pair<double, int> maxstep(const Eigen::VectorXd & f, const Eigen::VectorXd & a, const Eigen::VectorXd & delta_f, const Eigen::VectorXd & delta_a, int d);

		// Builds f vector from C_ values.
		Eigen::VectorXd transferTof(Eigen::VectorXd f, Eigen::VectorXd & vector);

	private:

		// Contains indices which have been already resolved.
		std::set<int> C_;

		// Contains indices which have not beed yet resolved.
		std::set<int> NC_;

		// A matrix of Ax+b=0
		const Eigen::MatrixXd * A_;

		// b vector of Ax+b=0;
		Eigen::VectorXd * b_;

		// Vector of forces which resolves the resting contact.
		Eigen::VectorXd * f_;
	};

}

#endif
